/**
 * @file pose.cc
 * Created on:  Oct 3, 2013
 *
 * @author: weipeng zhang
 */

#include <iostream>
#include <vector>
#include <cmath>
#include <cstdio>
#include <ctime>
#include <cstdlib>
#include <fstream>


#include <opencv2/opencv.hpp>

#include "hog-adapter.hpp"

#include "cblas.h"

using namespace cv;
using namespace std;

#define PART_SIZE 40  // 5*5 hog cell (8)
//170 * 200 mean size of PARSE image set
#define PART_NUM 18 //parse: 14, 26, buffy: 10, 18
#define IMG_NUM 944 // 944, 276
    
int PA[] = {0, 1, 2, 3, 4, 5, 6, 3, 8, 9, 2, 11, 12, 13, 14, 11, 16, 17};

struct Part {
    bool operator <(const Part &dst) const {
        return x < dst.x || x == dst.x && y < dst.y;
    }
    int x;
    int y;
    int type;
};

struct Pair {
    bool operator <(const Pair &dst) const {
        return x1 < dst.x1 || (x1 == dst.x1 && y1 < dst.y1) || (x1 == dst.x1 && y1 == dst.y1 && x2 < dst.x2) || (x1 == dst.x1 && y1 == dst.y1 && x2 == dst.x2 && y2 < dst.y2) ;
    }
    int x1;
    int y1;
    int x2;
    int y2;
};

struct Score {
    double score;
    Part part;
};

struct Model {
    /*static const char appear_file[] = "pose/buffy/appear_file"; // file contains appearance beta 
    static const char deform_file[] = "pose/buffy/deform_file"; // file contains deform beta
    static const char attr_file[] = "pose/buffy/attr_file"; // file contains all attributes beta
    */
    double ratio;
    double stride; // length for each step
    int part_num; //18
    int part_size; //40
    int positive_image_num; //944
    int negative_image_num; //1218
    int type_num; //6
    int appear_dim; //775 +_1
    int deform_dim; //4 + 1
    int attr_dim[3]; //96 * 3 + 1 = 289
    vector< vector<Part> >* pos_part; //positive image location&type
    char* pos_file;
    char* neg_file;
    /*static const char pos_file[] = "pose/buffy/BUFFY/im%.4d.jpg";
    static const char neg_file[] = "pose/INRIA/%.5d.jpg";
    */
    float* appear_beta; //18 * 6 * 776
    float* deform_beta; //17 * 6 * 5
    //float* attr_beta; //384 * 3 + 1 
    float* beta;
    int dim;
    map< int, float* >* attr_beta; //TODO need to initial
    map<int, vector<int> >* tree; //part's parent-child relation
};

// compute hog descriptor by pixel
void hog_pixel(vector<float> *desc , double x, double y, Mat &img);

// compute distance between two parts
void part_distance(double *dis, double x1, double y1, double x2, double y2);

// extract an image part, center is (x,y), bbox is 40*40
Mat img_part(Mat &img, double x, double y); 

void print_feature(int argc, char **argv);
int relative_location(int argc, char **argv); 
int relative_location_26(int argc, char **argv); 
void negative_feature();

vector<float> appear_feature(Mat &img, vector<Part> &pose);
vector<float> deform_feature(Mat &img, vector<Part> &pose);
// clothing attribute
vector<float> attribute_feature(Mat &img, vector<Part> &pose, const char* attr_type);

// clothing attribute for SVM training
void attribute_svm_feature(Mat &img, double* location, const char* attr);

// normalize vector
void my_normalize(Mat& src, Mat &dst);

// calculate color histogram
void calc_hist(Mat &img, double x, double y, vector<Mat>& res);

// inference, pictorial structure
vector<Part> pictorial_structure(Mat &img, Model &model, double threshold);

// computer local score for each part by equation (6)
void local_score(Mat &img, Model &model, int node, vector<Score>* score_set, vector< map< Part, vector<Part> > >* part_tree_set, int flag, map< Pair, vector<float> >* deform_set);

// computer attibuter value
int get_attribute(Mat &img, vector<Part> &pose, Model &model, const char* attr);

//inference algorithm 
vector<Part> inference(Mat &img, Model &model);

// pegasos algorithm
void update_model_with_pegasos(float* w, const int dim,
                               const float lambda /*=0.001*/,
                               const size_t K /*=1*/,
                               const vector<pair<vector<float>*, int> >& F);


/**
 * @brief compute execute time of program
 */
void exec_time() {
    clock_t begin, end;
    double time_spent;
    begin = clock();
    /* here, your program */
    end = clock();
    time_spent = (double)(end - begin) * 1000.0 / CLOCKS_PER_SEC;
}


/**
 * @brief pre computing the deform feature
 */
void deform_optimize(const Model& model, const Mat& img, map< long long , vector<float> >* deform_set) {
    clock_t begin, end;
    double time_spent;
    begin = clock();
    double dis[4] = {0};
    for (int i = 0; i < img.cols; i+=model.stride) {
        for (int j = 0; j < img.rows; j+=model.stride) {
            for (int s = 0; s < img.cols; s+=model.stride) {
                for (int t = 0; t < img.rows; t+=model.stride) {
                    long long deform_id = i << 30 | j << 20 | s << 10 | t;
                    part_distance(dis, i, j, s, t);
                    vector<float> deform;
                    deform.insert(deform.end(), dis, dis+4);
                    deform_set->insert( pair< long long, vector<float> >(deform_id, deform) );
                }
            }
        }
    }
    end = clock();
    time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
    cout << "deform_optimize time is " << time_spent << "s" << endl;
}



/**
 * @brief feature * beta
 */
double vector_multi(vector<float> feature, vector<float> beta) {
    if (feature.size() != beta.size()) {
        perror("feature and beta length mismatch.\n");
        return 0;
    }
    int len = feature.size();
    double sum = 0.0;
    for (int i = 0; i < len; i++) {
        sum += feature[i] * beta[i];
    }
    return sum;
}

/**
 * @brief feature * beta
 */
double vector_multi_v2(vector<float> feature, float* beta, int len) {
    if (feature.size() != len) {
        perror("feature and beta length mismatch.\n");
        return 0;
    }
    double sum = 0.0;
    for (int i = 0; i < len; i++) {
        sum += feature[i] * beta[i];
    }
    return sum;
}


void* mymalloc(size_t sz) {
    void* result = malloc(sz);
    assert(result);
    memset(result, 0, sz);
    return result;
}

int relative_location_26(int argc, char **argv) {
    ifstream ifs(argv[1]);

    //int pa[PART_NUM] = {0, 1, 2, 3, 4, 5, 6, 3, 8, 9, 10, 11, 12, 13, 2, 15, 16, 17, 18, 15, 20, 21, 22, 23, 24, 25};
    int pa[PART_NUM] = {0, 1, 2, 3, 4, 5, 6, 3, 8, 9, 2, 11, 12, 13, 14, 11, 16, 17};

    double part_location[2 * PART_NUM];
    for (int f = 1; f <= IMG_NUM; f++) {
        for (int l = 0; l < 2 * PART_NUM; l++) {
            ifs >> part_location[l];
        }

        for (int p = 0; p < PART_NUM; p++) {
            if (pa[p] == 0) {
                int r = 0;
                while (pa[r] != (p + 1)) {
                    r += 1;
                }
                cout << part_location[2 * r]  - part_location[2 * p] << " " << part_location[2 * r + 1] - part_location[2 * p + 1] << " ";
            } else {
                cout << part_location[2 * p]  - part_location[2 * pa[p - 1]] << " " << part_location[2 * p + 1] - part_location[2 * pa[p-1] + 1] << " ";
            }
        }

        cout << endl;
    }
}




int relative_location(int argc, char **argv) {
    ifstream ifs(argv[1]);

    int part_parent[14] = {1, 2, 8, 9, 3, 4, 7, 8, 12, 12, 9, 10, 13, 12};

    double part_location[2 * 14];
    for (int f = 1; f <= 305; f++) {
        for (int l = 0; l < 2 * 14; l++) {
            ifs >> part_location[l]; 
        }

        // lank lkne lhip rhip rkne rank lwr lelb lsho rsho relb rwr hbot htop
        // 0    2    4    6    8    10   12  14   16   18   20   22  24   26
        // lank wrt lkne
        cout << part_location[0] - part_location[2] << " " << part_location[1] - part_location[3] << "\t";
        // lkne wrt lhip
        cout << part_location[2] - part_location[4] << " " << part_location[3] - part_location[5] << "\t";
        // lhip wrt lsho
        cout << part_location[4] - part_location[16] << " " << part_location[5] - part_location[17] << "\t";
        // lwr wrt lelb
        cout << part_location[12] - part_location[14] << " " << part_location[13] - part_location[15] << "\t";
        // lelb wrt lsho
        cout << part_location[14] - part_location[16] << " " << part_location[15] - part_location[17] << "\t";
        // lsho wrt hbot
        cout << part_location[16] - part_location[24] << " " << part_location[17] - part_location[25] << "\t";
        // hbot wrt htop
        cout << part_location[24] - part_location[26] << " " << part_location[25] - part_location[27] << "\t";
        // rsho wrt hbot
        cout << part_location[18] - part_location[24] << " " << part_location[19] - part_location[25] << "\t";
        // relb wrt rsho 
        cout << part_location[20] - part_location[18] << " " << part_location[21] - part_location[19] << "\t";
        // rwr wrt relb
        cout << part_location[22] - part_location[20] << " " << part_location[23] - part_location[21] << "\t";
        // rhip wrt rsho
        cout << part_location[6] - part_location[18] << " " << part_location[7] - part_location[19] << "\t";
        // rkne wrt rhip
        cout << part_location[8] - part_location[6] << " " << part_location[9] - part_location[7] << "\t";
        // rank wrt rkne
        cout << part_location[10] - part_location[8] << " " << part_location[11] - part_location[9] << "\t";
        // htop wrt hbot  ** for head
        cout << part_location[24] - part_location[26] << " " << part_location[25] - part_location[27] << "\t";

        cout << endl;
    }
    return 0;
}

// generate negative feature vector for SVM train
void negative_feature() {
    int i, j, k = 1;
    char file_name[30];
    vector<float> desc;
    double dis[4] = {0};
    double part_location[2 * PART_NUM];
    //int part_parent[14] = {1, 2, 8, 9, 3, 4, 7, 8, 12, 12, 9, 10, 13, 12};
    const char* attr_type = "sleeve";

    for (int f = 615; f <= 1832; f++) {
        int r = 1;
        sprintf(file_name, "pose/INRIA/%.5d.jpg", f);
        Mat img = imread(file_name);
        //generate part location randomly
        for (i = 0; i < 2 * PART_NUM; i+=2) {
            srand(k);
            part_location[i] = (double)(rand() % img.cols);
            srand(k + 1);
            part_location[i + 1] = (double)(rand() % img.rows);
            k += 2;
        }
        //attribute_feature(file_name, attr_type, part_location);
        attribute_svm_feature(img, part_location, attr_type);

        //print the feature vector
        // appearance model
        /*cout << "-1 ";
        hog_pixel(&desc, part_location[part_num * 2], part_location[part_num * 2 + 1], img);
        for (j = 0; j < desc.size(); j++) {
            cout << r << ":" << desc[j] << " ";
            r++;
        }*/

        // deformation model
        /*int parent = 0;
        for ( i = 0; i < 14; i++) {
            parent = part_parent[i];
            part_distance(dis, part_location[i * 2], part_location[i * 2 + 1], part_location[parent * 2], part_location[parent * 2 + 1]);
            for (k = 0; k < 4; k++) {
                cout << r << ":" << dis[k] << " ";
                r++;
            }
        }
        */
    }
}

/**
 * @brief compute the hog feature for each part of an image
 * @return an concatenate feature vector
 */
vector<float> appear_feature(Mat &img, vector<Part> &pose) {
    vector<float> feature, desc;
    for (int i = 0; i < pose.size(); i++) {
        hog_pixel(&desc, pose[i].x, pose[i].y, img);
        desc.push_back(1.0); //@attention add the bias
        feature.insert(feature.end(), desc.begin(), desc.end());
    }
    return feature;
}



/**
 * @brief compute the deformation feature for each part-pair of an image
 * @brief [dx, dy, dx^2, dy^2]
 * @return an concatenate feature vector
 */
vector<float> deform_feature(Mat &img, vector<Part> &pose) {
    int pa[PART_NUM] = {0, 1, 2, 3, 4, 5, 6, 3, 8, 9, 2, 11, 12, 13, 14, 11, 16, 17};
    vector<float> feature;
    double dis[4] = {0};
    int parent;
    cout << "deform feature pose size is: " << pose.size() << endl;
    for (int i = 1; i < pose.size(); i++) {
        parent = pa[i - 1];
        part_distance(dis, pose[i].x, pose[i].y, pose[parent].x, pose[parent].y);
        cout << "deform feature test flag 1: " << i << endl;
        feature.insert(feature.end(), dis, dis + 4);
        feature.push_back(1.0);
        cout << "deform feature test flag 2: " << i << endl;
    }
    return feature;
}

/**
 * @brief generate the svm feature about clothing attribute
 */
void attribute_svm_feature(Mat &img, double* location, const char* attr) {
    int rel_part[] = {3, 4, 5, 6, 7, 11, 12, 13, 14, 15};
    if (attr == "sleeve") {
        int i, len;
        for (int p = 0; p < 10; p++) {
            vector<Mat> each_part;
            calc_hist(img, location[2 * rel_part[p] - 2], location[2 * rel_part[p] - 1], each_part);
            //@warning I am not sure rows or cols
            len = each_part[0].rows;
            for (i = 0; i < 3; i++) {
                my_normalize(each_part[i], each_part[i]);
                for (int j = 0; j < len; j++) {
                    //cout << i * len + (j + 1) << ":" << each_part[i].at<float>(j) << " ";
                    cout << each_part[i].at<float>(j) << " ";
                }
            }
            cout << "\t";
        }
        cout << endl;
    }
    //TODO
}

// generate the feature vector for SVM train model
void print_feature(int argc, char **argv) {
    if (argc != 3) {
        printf("Usage: %s location_labels type_labels\n", argv[0]);
        exit(1);
    }

    ifstream ifs(argv[1]);
    ifstream ifs_t(argv[2]);
    //int part_num = atoi(argv[3]) - 1;
    //int type_num = atoi(argv[4]) - 1;


    //int part_parent[14] = {1, 2, 8, 9, 3, 4, 7, 8, 12, 12, 9, 10, 13, 12};
    int pa[PART_NUM] = {0, 1, 2, 3, 4, 5, 6, 3, 8, 9, 2, 11, 12, 13, 14, 11, 16, 17};

    double part_location[2 * PART_NUM];
    int part_type[PART_NUM];
    vector<float> desc;
    double dis[4] = {0};
    char file_name[30];
    const char* attr_type = "sleeve";
    int rand_flag = 0;
    //int parent = pa[part_num] - 1;
    Mat img;
    for (int f = 1; f <= IMG_NUM; f++) {
        for (int l = 0; l < 2 * PART_NUM; l++) {
            ifs >> part_location[l]; 
        }
        for (int l = 0; l < PART_NUM; l++) {
            ifs_t >> part_type[l];
        }
        sprintf(file_name, "pose/buffy/BUFFY/im%.4d.jpg", f);
        img = imread(file_name);
        //attribute_feature(img, attr_type, part_location);
        attribute_svm_feature(img, part_location, attr_type);

        //appear_feature(file_name, &part_location, part_num, type_num);

        //deform_feature(&part_location, &pa);
    }
}

Mat img_part(Mat &img, double x, double y) {
    int start_x = (x - 20) >= 0 ? ceil(x - 20) : 0;
    int end_x = (x + 20) >= img.cols ? img.cols : ceil(x + 20);
    int start_y = (y - 20) >= 0 ? ceil(y - 20) : 0;
    int end_y = (y + 20) >= img.rows ? img.rows : ceil(y + 20);
    Mat part_img = img.colRange(start_x, end_x);
    part_img = part_img.rowRange(start_y, end_y);
    Mat _part_img;
    resize(part_img, _part_img, Size(40, 40), 0, 0, INTER_LINEAR);
    return _part_img;
}


void hog_pixel(vector<float> *desc, double x, double y, Mat &img) {
    Mat part_img = img_part(img, x, y);
    jieshen::HOG_ADAPTER hog;
    hog.setImage(&part_img);
    hog.extractHOGFeature(desc);
}


void part_distance(double *dis, double x1, double y1, double x2, double y2) {
    dis[0] = x1 - x2;
    dis[1] = (x1 - x2) * (x1 - x2);
    dis[2] = y1 - y2;
    dis[3] = (y1 - y2) * (y1 - y2);
}


// compute the color histogram for each image part
void calc_hist(Mat &img_p, double x, double y, vector<Mat>& res) {
    Mat img = img_p;
    if (x > 0 && y > 0) {
        img = img_part(img_p, x, y);
    }
    vector<Mat> bgr_planes;
    split(img, bgr_planes);
    int hist_size = 32;
    float range[] = {0, 256};
    const float* hist_range = {range};
    bool uniform = true;
    bool accumulate = false;
    Mat b_hist, g_hist, r_hist;
    calcHist( &bgr_planes[0], 1, 0, Mat(), b_hist, 1, &hist_size, &hist_range, uniform, accumulate );
    calcHist( &bgr_planes[0], 1, 0, Mat(), g_hist, 1, &hist_size, &hist_range, uniform, accumulate );
    calcHist( &bgr_planes[0], 1, 0, Mat(), r_hist, 1, &hist_size, &hist_range, uniform, accumulate );
    
    Mat result[] = {b_hist, g_hist, r_hist};
    res.insert(res.begin(), result, result + 3);
}
    
// normalize by the summarization
// Param: 1*N
void my_normalize(Mat& src, Mat &dst) {
    float sum = 0.0f;
    for (int i = 0; i < src.rows; i++) {
        sum += src.at<float>(i);
    }

    for (int i = 0; i < dst.rows; i++) {
        dst.at<float>(i) /= sum;
    }
}

    
// add "Mat num" to "Mat src"
void mat_add(vector<Mat> src, vector<Mat> num) {
    for (int i = 0; i < src[0].rows; i++) {
        for (int j = 0; j < 3; j++) {
            src[j].at<float>(i) += num[j].at<float>(i);
        }
    }
}

// print attribute feature for each big part
vector<float> print_each_part(Mat& img, vector<Part> &pose, int* part, int len) {
    /* @warning uninitialized vector */
    vector<float> feature;
    vector<Mat> each_part;
    calc_hist(img, pose[part[0] - 1].x, pose[part[0] - 1].y, each_part);

    for (int i = 1; i < len; i++) {
        vector<Mat> tmp_part;
        calc_hist(img, pose[part[i] - 1].x, pose[part[i] - 1].y, tmp_part);
        mat_add(each_part, tmp_part);
    }


    for (int i = 0; i < 3; i++) {
        my_normalize(each_part[i], each_part[i]);
        for (int j = 0; j < each_part[i].rows; j++) {
            /* @warning vector push_back, call much more times */
            feature.push_back(each_part[i].at<float>(j));
            //printf("%f ", each_part[i].at<float>(j));
        }
    }
    return feature;
}

// compute attribute feature
vector<float> attribute_feature(Mat &img, vector<Part>  &pose, const char* attr_type) {
    vector<float> feature, desc;
    if (attr_type == "sleeve") {
        int part[] = {3, 4, 5, 6, 7, 11, 12, 13, 14, 15};
        feature = print_each_part(img, pose, part, 10);

        /*int part_lu[] = {3, 4, 5};
        // left upper
        desc = print_each_part(img, pose, part_lu);
        feature.insert(feature.end(), desc.begin(), desc.end());

        // left low
        int part_ll[] = {5, 6, 7};
        desc = print_each_part(img, pose, part_ll);
        feature.insert(feature.end(), desc.begin(), desc.end());

        // right upper
        int part_ru[] = {11, 12, 13};
        desc = print_each_part(img, pose, part_ru);
        feature.insert(feature.end(), desc.begin(), desc.end());

        // right low
        int part_rl[] = {13, 14, 15};
        desc = print_each_part(img, pose, part_rl);
        feature.insert(feature.end(), desc.begin(), desc.end());
        */
    }
    return feature;
}

/**
 * @brief generate attribute-pose combined feature, attr \otimes <1, 0, 0>
 */
void otimes_feature(vector<float> &feature, int value) {
    int len = feature.size();
    int* zeros = (int*)mymalloc(len);
    memset(zeros, 0, len);
    if (value == 0) {
        feature.insert(feature.end(), zeros, zeros + len);
        feature.insert(feature.end(), zeros, zeros + len);
    } else if (value == 1) {
        feature.insert(feature.begin(), zeros, zeros + len);
        feature.insert(feature.end(), zeros, zeros + len);
    } else if (value == 2) {
        feature.insert(feature.begin(), zeros, zeros + len);
        feature.insert(feature.begin(), zeros, zeros + len);
    } else {
        perror("Wrong attribute value.\n");
    }
    free(zeros);
}


/**
 * @brief initial model parameters by seperately trained beta
 * 
 */
void init_model(Model &model) {
    cout << "init model" << endl;
    int sleeve_part[] = {3, 4, 5, 6, 7, 11, 12, 13, 14, 15};
    model.pos_file = "pose/buffy/BUFFY/im%.4d.jpg";
    model.neg_file = "pose/INRIA/%.5d.jpg";
    model.part_size = 40;
    model.part_num = 18;
    model.type_num = 6;
    model.ratio = 0.5;
    model.stride = model.ratio * model.part_size;
    model.positive_image_num = 944;
    model.negative_image_num = 1218;
    model.appear_dim = 776;
    model.deform_dim = 5;
    model.attr_dim[0] = 193; //sleeve, 96 * 2 + 1
    model.attr_dim[1] = 0; //
    model.attr_dim[2] = 0; //
    model.appear_beta = (float*)mymalloc(model.part_num * model.type_num * model.appear_dim * sizeof(float) );
    model.deform_beta = (float*)mymalloc((model.part_num - 1) * model.type_num * model.deform_dim * sizeof(float) );
    cout << "alloc appear and deform beta" << endl;
    model.dim = model.part_num * model.type_num * model.appear_dim + (model.part_num - 1) * model.type_num * model.deform_dim + model.attr_dim[0];
    //read location&type of positive image
    ifstream ifs("pose/buffy/norm_labels");
    //ifstream ifs_s("pose/buffy/labels");
    ifstream ifs_t("pose/buffy/part_type_labels");
    //model.pos_part = (vector< vector<Part> >*)mymalloc( sizeof(vector< vector<Part> >*) );
    model.pos_part = new vector< vector<Part> >;
    (model.pos_part)->resize(model.positive_image_num);
    cout << "pos_part resize\n";
    double x, y, type;
    for (int i = 0; i < model.positive_image_num; i++) {
        //model.pos_part->at(i).resize(model.part_num);
        for (int j = 0; j < model.part_num; j++) {
            Part part;
            ifs >> x;
            ifs >> y;
            //ifs_s >> part.lx;
            //ifs_s >> part.ly;
            ifs_t >> type;
            part.x = (int)x;
            part.y = (int)y;
            part.type = (int)type;
            ( model.pos_part->at(i) ).push_back(part);
        }
        //(model.pos_part)->push_back(human);
    }


    cout << "location and type labels read over" << endl;

    //Read parameter data from file
    //int length;
    ifstream ifs_a("pose/buffy/feature.appear");
    int length = model.part_num * model.type_num * model.appear_dim;
    model.appear_beta = (float*)mymalloc( length * sizeof(float) );
    for (int i = 0; i < length; i++) {
        ifs_a >> model.appear_beta[i];
        //cout << length << " *** " << i << " = " << model.appear_beta[i] << endl;
    }
    cout << "appear feature read OK." << endl;
    ifstream ifs_b("pose/buffy/feature.deform");
    length = (model.part_num - 1) * model.type_num * model.deform_dim;
    model.deform_beta = (float*)mymalloc( length * sizeof(float) );
    for (int i = 0; i < length; i++) {
        ifs_b >> model.deform_beta[i];
    }
    cout << "deform feature read OK." << endl;

    cout << "attr: sleeve start" << endl;
    /*model.attr_beta = new map< int, float* >;
    ifstream ifs_c("pose/buffy/feature.sleeve");
    cout << "hello world\n";
    length = model.attr_dim;
    for (int p = 0; p < 10; p++) {
        cout << "sleeve begin*** " << p << endl;
        float tmp[193];
        for (int i = 0; i < 193; i++) {
            ifs_c >> tmp[i];
        }
        cout << "sleeve end*** " << p << " = " << tmp[0] << endl;
        (model.attr_beta)->insert( pair<int, float*>(sleeve_part[p], tmp) );
    }*/
    model.attr_beta = new map< int, float* >;
    ifstream ifs_c("pose/buffy/feature.sleeve");
    length = model.attr_dim[0];
    float tmp[1930];
    for (int i = 0; i < 1930; i++) {
        ifs_c >> tmp[i];
    }
    (model.attr_beta)->insert( pair<int, float*>(0, tmp) );
    cout << "attr: sleeve read OK" << endl;

    // initial the parent-child map
    model.tree = new map< int, vector<int> >;
    //model.tree = ( map< int, vector<int> >* )mymalloc( sizeof( map< int, vector<int> >*) );
    map< int, vector<int> >::iterator it;
    for (int i = 1; i <= model.part_num; i++) {
        cout << "map tree, start insert\n";
        if (PA[i - 1] == 0) {  // skip 0
            continue;
        }
        if ((model.tree)->count( PA[i - 1] ) > 0) {
            cout << "map tree, already has\n";
            it = (model.tree)->find( PA[i - 1] );
            it->second.push_back(i);
        } else {
            vector<int> tmp;
            tmp.push_back(i);
            (model.tree)->insert(pair<int, vector<int> >(PA[i - 1], tmp));
        }
    }

    cout << "hello world" << endl;

    //initial beta, combine all parameters
    //TODO
    //model.beta
}

/**
 * @brief hog feature * appear_beta
 */
double hog_score(Mat &img, Model &model, Part &part, int part_id) {
    clock_t begin, end;
    double time_spent;
    begin = clock();
    vector<float> hog_feature;   
    hog_pixel(&hog_feature, part.x, part.y, img);
    int start = part_id * model.type_num * model.appear_dim + part.type * model.appear_dim;
    int len = hog_feature.size();
    double sum = 0.0;
    for (int i = start + 1; i <= start + len; i++) {
        sum += hog_feature[i - start - 1] * model.appear_beta[i];
    }
    end = clock();
    cout << "hog score, part( " << part.x <<"," << part.y <<","<< part.type << "), sum is" << sum <<  ", spent time (s) is " << (double)(end - begin) / CLOCKS_PER_SEC << endl;
    return sum;
}

/**
 * @brief max{ score(z_i) + deform(z_i, z_j) }
 */
double get_max_score(Model &model, vector<Score> &child, Part &parent, int child_node, map< long long, vector<float> >* deform_set, Part* max_part) {
    clock_t begin, end;
    double time_spent;
    begin = clock();

    int len = child.size();
    int start, i, j;
    double sum = 0.0, tmp_sum;
    //Part max_part; //record the max part which related max score

    //double dis[4] = {0};
    //part_distance(dis, child[0].part.x, child[0].part.y, parent.x, parent.y);
    long long deform_id = child[0].part.x << 30 | child[0].part.y << 20 | parent.x << 10 | parent.y;
    vector<float> dis = deform_set->find(deform_id)->second;

    start = (child_node-1) * model.type_num * model.deform_dim + child[0].part.type * model.deform_dim;
    for (j = start; j < start + model.deform_dim - 1; j++) {
        sum += dis[j - start] * model.deform_beta[j];
    }
    sum += 1 * model.deform_beta[j];
    printf("j is %d, model deform beta is %f, len is %d.\n", j, model.deform_beta[j], len);
    sum += child[0].score;
    max_part->x = child[0].part.x;
    max_part->y = child[0].part.y;
    max_part->type = child[0].part.type;
    for (i = 1; i < len; i++) {
        tmp_sum = 0.0;

        deform_id = child[i].part.x << 30 | child[i].part.y << 20 | parent.x << 10 | parent.y;
        dis = deform_set->find(deform_id)->second;

        //part_distance(dis, child[i].part.x, child[i].part.y, parent.x, parent.y);
        start = (child_node-1) * model.type_num * model.deform_dim + child[i].part.type * model.deform_dim;
        for (j = start; j < start + model.deform_dim - 1; j++) {
            tmp_sum += dis[j - start] * model.deform_beta[j];
        }
        tmp_sum += 1 * model.deform_beta[j];
        tmp_sum += child[i].score;
        //cout << "tmp_sum is " << tmp_sum << endl;
        if (tmp_sum > sum) {
            sum = tmp_sum;
            //max_part = &(child[i].part);
            max_part->x = child[i].part.x;
            max_part->y = child[i].part.y;
            max_part->type = child[i].part.type;
        }
    }

    end = clock();
    time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

    printf("child_node: %d, max location is: (%d, %d), type is %d, sum is %f, parent location(%d, %d, %d), spent time is %f.\n", child_node, max_part->x, max_part->y, max_part->type, sum, parent.x, parent.y, parent.type, time_spent);
    return sum;
}

/**
 * @brief check if an array contains this number
 */
int is_contain(int num, const int* array, int len) {
    for (int i = 0; i < len; i++) {
        if (array[i] == num) {
            return 1;
        }
    }
    return 0;
}

/**
 * @brief the key contribution of our work, clothing attribute score to pose estimation
 */
double extra_attr_score(Mat &img, Model &model, int node, Part &part, const char* attr) {
    double sum = 0.0;
    if (attr == "sleeve") {
        int rel_part[] = {3, 4, 5, 6, 7, 11, 12, 13, 14, 15};
        if (is_contain(node, rel_part, 10)) {
            map<int, float*>* beta_set = model.attr_beta;
            float* beta = beta_set->find(node)->second;
            vector<float> feature, tmp;
            vector<Mat> each_part;
            calc_hist(img, part.x, part.y, each_part);
            //@warning I am not sure rows or cols
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < each_part[i].rows; j++) {
                    feature.push_back(each_part[i].at<float>(j));
                }
            }
            feature.push_back(1.0); //add bias
            
            int len = feature.size();
            double tmp_sum = -1.1;
            for (int i = 0; i < 3; i++) {
                sum = 0.0;
                tmp = feature;
                otimes_feature(tmp, i);
                for (int j = 0; j < len; j++) {
                    sum += feature[j] * beta[j];
                }
                if (sum > tmp_sum) {
                    tmp_sum = sum;
                }
            }
            sum = tmp_sum;
        }
    }
    return sum;
}

/**
 * @brief function for Part compare
 */
bool cmp_part(Part p1, Part p2) {
    return p1.x > p2.x;
}
                               

/**
 * @brief just use DFS to compute pose score
 * @brief hog_score(z_i) + \sum { get_max_score(child_set) }
 * @param vector< map<Part, vector<Part>> >* part_tree: record the trace
 */
void local_score(Mat &img, Model &model, int node, vector< vector<Score> >* score_set, vector< map< Part, vector<Part> > >* part_tree_set, int flag, map< long long, vector<float> >* deform_set) {
    cout << "Now part node is " << node << endl;
    //not leaf node
    if ( (model.tree)->count(node) > 0 ) {
        vector<int> childs = (model.tree)->find(node)->second;
        for (int i = 0; i < childs.size(); i++) {
            cout << "childs " << childs[i] << " size," << score_set->at(childs[i] - 1).size() << endl;
            if (score_set->at(childs[i] - 1).size() == 0) {
                cout << "not leaf node\n";
                local_score(img, model, childs[i], score_set, part_tree_set, flag, deform_set);
            }
        }
    }

    cout << "node flag 1" << endl;

    //compute score
    vector<Score>* parent = &( score_set->at(node - 1) );
    
    cout << "node flag 2" << endl;
    // function point for map's key compare
    //bool(*fn_pt)(Part, Part) = cmp_part;
    //map< Part, vector<Part> , bool(*)(Part, Part) > part_tree(fn_pt); 
    map< Part, vector<Part> >* part_tree = &(part_tree_set->at(node - 1)); 
    double tmp, max = 0.0;
    cout << "node flag 3" << endl;

    /*** compute program time ***/
    clock_t begin, end;
    double time_spent;
    /******/
    begin = clock();
    for (int i = 0; i < img.cols; i += model.stride) {
        for (int j = 0; j < img.rows; j += model.stride) {
            for (int t = 0; t < model.type_num; t++) {
                Part tmp_part = {i, j, t};
                tmp = 0.0;
                tmp += hog_score(img, model, tmp_part, node - 1);
                //cout << "node " << node << ", tmp is " << tmp << "\n";
                //not leaf node
                if ( (model.tree)->count(node) > 0 ) {
                    cout << "node " << node << ", has childs\n";
                    vector<int>& childs = (model.tree)->find(node)->second;
                    vector<Part> tmp_vec(childs.size());
                    for (int c = 0; c < childs.size(); c++) {
                        tmp += get_max_score(model, score_set->at(childs[c] - 1), tmp_part, childs[c], deform_set, &tmp_vec[c]);
                    }
                    part_tree->insert( pair< Part, vector<Part> >(tmp_part, tmp_vec) );
                }
                //add attribute score, @warning now only sleeve attribtue
                /*if (flag == 1) {
                    tmp += extra_attr_score(img, model, node, tmp_part, "sleeve");
                }*/
                
                if (tmp > max) {
                    max = tmp;
                }
                cout << "node " << node << ", tmp is " << tmp << "\n";
                //cout << "part = " << node << "; i = " << i << "; j = " << j << "; t = " << t << endl;
                Score score = {tmp, tmp_part};
                parent->push_back(score);
            }
        }
    }
    end = clock();
    cout << "node " << node << ", spent time (s) is " << (double)(end - begin) / CLOCKS_PER_SEC << endl;
}

/**
 * @brief parse the human part which related to max score
 */
void parse_human_part(Model &model, vector<Score> root, int node, vector< map< Part, vector<Part> > >* part_tree_set, vector<Part>* candidates) {
    // initialize the whole part tree
    int len;
    if (node == 1) {
        len = root.size();
        double score = 0;
        Part max_part;
        for (int i = 0; i < len; i++) {
            //cout << "root[" << i << "] score = " << root[i].score << endl;
            printf("root[%d] score = %f\n", i, root[i].score);
            if (root[i].score > score) {
                score = root[i].score;
                max_part = root[i].part;
            }
        }
        candidates->at(node - 1).x = max_part.x;
        candidates->at(node - 1).y = max_part.y;
        candidates->at(node - 1).type = max_part.type;
        cout << "Max part: x = " << max_part.x << "; y = " << max_part.y << "; type = " << max_part.type << ";\n";
    }

    cout << "part = " << node << endl;
    cout << "x = " << candidates->at(node - 1).x << "; y = " << candidates->at(node - 1).y << "; type = " << candidates->at(node - 1).type << ";\n";
    if ( (model.tree)->count(node) > 0 ) {
        vector<int>& childs = (model.tree)->find(node)->second;
        vector<Part> max_childs;
        if ( part_tree_set->at(node - 1).count( candidates->at(node - 1) ) > 0 ) {
            max_childs = part_tree_set->at(node - 1).find( candidates->at(node - 1) )->second;
        } else {
            cout << "part node's value is null\n";
        }
        cout << "human data ready\n";
        len = childs.size();
        for (int i = 0; i < len; i++) {
            candidates->at( childs[i] - 1 ) = max_childs[i];
            parse_human_part(model, root, childs[i], part_tree_set, candidates);
        }
    }
}

/**
 * @brief 1. compute the leaf node's score, only hog feature and W
 * @brief 2. for each possible location and type of child node, compute the score for parent node
 * @brief 3. get the max score root node by threshold
 * @prama Mat &img: image search space
 * @prama Model &model: beta parameters
 * @parma double threshold: select candidates above this threshold
 * @return human part
 */
vector<Part> pictorial_structure(Mat &img, Model &model, double threshold) {
    cout << "ps start\n";
    vector< vector<Score> >* score_set = new vector< vector<Score> >;
    score_set->resize(model.part_num);
    for (int i = 0; i < model.part_num; i++) {
        score_set->at(i).resize(0);
    }
    cout << "test flag 1\n";
    //vector< map< Part, vector<Part>, bool(*)(Part, Part) > > part_tree_set[model.part_num];
    //vector< map< Part, vector<Part>, bool(*)(Part, Part) > >* part_tree_set = new vector< map< Part, vector<Part>, bool(*)(Part, Part) > >;
    vector< map< Part, vector<Part> > >* part_tree_set = new vector< map< Part, vector<Part> > >;
    part_tree_set->resize(model.part_num);
    cout << "test flag 2\n";

    map< long long, vector<float> >* deform_set = new map< long long, vector<float> >;
    deform_optimize(model, img, deform_set);

    local_score(img, model, 1, score_set, part_tree_set, 0, deform_set);

    cout << "parse start\n";
    vector<Part> candidates;
    candidates.resize(model.part_num);
    //parse the human part which related to max score
    cout << "root score size = " << score_set->at(0).size() << endl;
    parse_human_part(model, score_set->at(0), 1, part_tree_set, &candidates);
    return candidates;
}

/**
 * @brief compute attribute value, <pose, attr> * beta
 * @brief just compute the <attr> * beta
 * @brief choose the attribute value related to max score
 * @param pose: human pose part
 * @param beta: model parameters
 * @return attribute value
 */
int get_attribute(Mat &img, vector<Part> &pose, Model &model, const char* attr) {
    int value = 0;
    if (attr == "sleeve") {
        vector<float> feature = attribute_feature(img, pose, attr);
        double score = 0.0, tmp_score;
        otimes_feature(feature, 0);
        score = vector_multi_v2(feature, (model.attr_beta)->find(0)->second, model.attr_dim[0]); //TODO
        for (int i = 1; i < 3; i++) {
            otimes_feature(feature, i);
            tmp_score = vector_multi_v2(feature, (model.attr_beta)->find(0)->second, model.attr_dim[0]);
            if (score < tmp_score) {
                score = tmp_score;
                value = i;
            }
        }
    }
    return value;
}

/**
 * @brief compute human part by adding parameter attribute
 * @param search space image I, model parameter beta, attribute value
 * @return pose human part's location and type
 */
vector<Part> my_pictorial_structure(Mat &img, Model &model, int attribute_value, double threshold) {
    vector< vector<Score> >* score_set = new vector< vector<Score> >;
    score_set->resize(model.part_num);

    vector< map< Part, vector<Part> > >* part_tree_set = new vector< map< Part, vector<Part> > >;
    part_tree_set->resize(model.part_num);

    map< long long, vector<float> >* deform_set = new map< long long, vector<float> >;
    deform_optimize(model, img, deform_set);
    //TODO modify part_tree_set
    local_score(img, model, 0, score_set, part_tree_set, 1, deform_set);

    vector<Part> candidates;
    //parse the human part which related to max score
    parse_human_part(model, score_set->at(0), 0, part_tree_set, &candidates);
    return candidates;
}

/**
 * @brief compute score with <pose, attr> * beta
 * @param vector<Part> pose: human part location and type
 * @param Model &model: parameters which trained by latent SVM
 * @param const char** attr_set: attribute type set
 * @return int score: whole score
 */
double compute_score(Mat &img, vector<Part> pose, Model &model, const char* attr, int attr_type) {
    double score = 0.0;
    if (attr == "sleeve") {
        vector<float> feature;
        //appear score
        feature = appear_feature(img, pose);
        score += vector_multi_v2(feature, model.appear_beta, model.part_num * model.type_num * model.appear_dim);
        //deform score
        feature = deform_feature(img, pose);
        score += vector_multi_v2(feature, model.deform_beta, (model.part_num - 1) * model.type_num * model.deform_dim);
        //attr score
        feature = attribute_feature(img, pose, attr);
        otimes_feature(feature, attr_type);
        //score += vector_multi_v2(feature, model.attr_beta, model.attr_dim);
        //TODO add attr score
    }
    return score;
}

/**
 * @brief inference algorithm, use attribute improving performance of pose
 * @param I: image search space
 * @param beta: model parameters
 * @param attr_set: attribute set in our work
 * @return human part and attribute type
 */
vector<Part> inference(Mat &img, Model &model, int* attr_type){
    double threshold = 0.0;
    double old_score = 0.0, score = 0.0;
    int attr_type_tmp = 0;
    //@warning just only one attribute so far
    vector<Part> pose = pictorial_structure(img, model, threshold);
    do {
        attr_type_tmp = get_attribute(img, pose, model, "sleeve");
        old_score = compute_score(img, pose, model, "sleeve", attr_type_tmp);
        pose = my_pictorial_structure(img, model, attr_type_tmp, threshold);
        score = compute_score(img, pose, model, "sleeve", attr_type_tmp);
    } while (abs(old_score - score) < 10e-6);
    *attr_type = attr_type_tmp; //TODO
    return pose;
}

/**
 * @brief just test, pose estimation, no attribute
 */
vector<Part> inference_origin(Mat &img, Model &model){
    double threshold = 0.0;
    double old_score = 0.0, score = 0.0;
    vector<Part> pose = pictorial_structure(img, model, threshold);
    return pose;
}

/**
 * @brief combine get_attribute and compute_score
 * @return the feature which related highest score
 */
vector<float> max_attr_feature(Mat &img, vector<Part> &pose, Model &model, const char* attr) {
    cout << "max_attr_feature" <<  endl;
    vector<float> feature, desc, tmp_desc;
    if (attr == "sleeve") {
        //appear score
        desc = appear_feature(img, pose);
        feature.insert(feature.end(), desc.begin(), desc.end());
        //deform score
        desc = deform_feature(img, pose);
        feature.insert(feature.end(), desc.begin(), desc.end());
        //attr score
        desc = attribute_feature(img, pose, attr);
        double score = 0.0, tmp;
        int max_attr = 0;
        tmp_desc = desc;
        otimes_feature(tmp_desc, 0);
        //score = vector_multi_v2(tmp_desc, model.attr_beta, model.attr_dim);
        score = 0;
        for (int i = 1; i < 3; i++) {
            tmp_desc = desc;
            otimes_feature(tmp_desc, i);
            //tmp = vector_multi_v2(tmp_desc, model.attr_beta, model.attr_dim);
            tmp = 0;
            if (tmp > score) {
                score = tmp;
                max_attr = i;
            }
        }
        otimes_feature(desc, max_attr);
        feature.insert(feature.end(), desc.begin(), desc.end());
    }
    return feature;
}

/**
 * @param model: model parameters
 * @param pos: positive examples set
 * @return the latent variable's value which related to the highest score
 */
vector< vector<float> > relabel_positive(Model &model, const char* attr) {
    char file_name[30];
    Mat img;
    vector< vector<float> > max_attr_set;
    for (int f = 0; f < model.positive_image_num; f++)  {
        sprintf(file_name, model.pos_file, f + 1);
        img = imread(file_name);
        cout << file_name << img.rows << img.cols << endl;
        vector<float> tmp = max_attr_feature(img, (model.pos_part)->at(f), model, attr);         
        max_attr_set.push_back(tmp);
    }
    return max_attr_set;
}


/**
 * @brief get negative features
 */
vector< vector<float> > detect_negative(Model &model, const char* attr) {
    vector< vector<float> > neg;
    char file_name[30];
    Mat img;
    vector<Part> negative_pose_tmp;
    for (int f = 615; f < model.negative_image_num + 615; f++) {
        sprintf(file_name, model.neg_file, f);
        printf("%s\n", file_name);
        img = imread(file_name);
        negative_pose_tmp = inference_origin(img, model);
        vector<float> tmp = max_attr_feature(img, negative_pose_tmp, model, attr);         
        neg.push_back(tmp);
    }
    return neg;
}


/**
 * @brief pagasos solver algorithm to update \beta
 * @param pos: positive examples set
 * @param neg: negative examples set
 * @param attr_type_tmp: fixed attribute value
 * @param model: model parameters, and need to update
 */
void pagasos_svm(vector< vector<float> > &pos, vector< vector<float> > &neg, Model &model) {
    vector< pair<vector<float>*, int> > features;
    printf("update model with pegasos, construct feature\n");
    int len = pos.size();
    for (int i = 0; i < len; i++) {
        features.push_back(pair<vector<float>*, int>(&(pos[i]), 1));
    }
    len = neg.size();
    for (int i = 0; i < len; i++) {
        features.push_back(pair<vector<float>*, int>(&(neg[i]), -1));
    }
    printf("update model with pegasos\n");
    update_model_with_pegasos(model.beta, model.dim, 1.0, 5, features);
}

/**
 * @brief \sum{ sqr(old[i], new[i]) }
 */
double diff(Model &old_model, Model &model) {
    float* old_beta = old_model.beta;
    float* beta = model.beta;
    double sum = 0.0;
    for (int i = 0; i < model.dim; i++) {
        sum += (old_beta[i] - beta[i]) * (old_beta[i] - beta[i]);
    }
    return sum;
}


/**
 * @brief training algorithm, use Latent SVM to update \beta
 * @brief 1. Relabel positive examples: get_attribute(img, pose, model, attr)
 * @brief 2. Optimize beta: L_D(\beta, Z) = \frac{1}{2} \|\beta\|^2 + C \sum_{i=1}^{n}max(0, 1 - y_i f_{\beta}(x_i, Z(x_i))
 * @brief Pagosos SVM solver to minium this L_D(\beta, Z)
 * @param pos: positive image examples set
 * @param neg: negative image examples set
 * @param model: initialized model
 */
void training(Model &model) {
    vector< vector<float> > pos, neg;
    Model old_model;
    const char* attr = "sleeve";
    do {
        cout << "model start training\n";
        old_model = model;
        pos = relabel_positive(model, attr); //relabel positive examples
        cout << "relabel positive" << endl;
        neg = detect_negative(model, attr);
        cout << "detect negative" << endl;
        printf("pagosos_svm start\n");
        pagasos_svm(pos, neg, model); //optimize beta
        cout << "model end training\n";
    } while(diff(old_model, model) < 10e-6);
}

void free_model(Model &model) {
    free(model.appear_beta);
    free(model.deform_beta);
    free(model.attr_beta);
    free(model.pos_part);
    free(model.tree);
}

/**
 * evaluate pose estimation
 */
void eval_pcp() {}

void print_human_part(vector<Part> &human) {
    int len = human.size();
    for (int i = 0; i < len; i++) {
        cout << "Part " << i + 1 << ", " <<  human[i].x << " " << human[i].y << " " << human[i].type <<endl;
    }
}

void print_groundtruth(const Model& model, const int img_id) {
    vector<Part>& tmp = (model.pos_part)->at(img_id);
    for (int i = 0; i < model.part_num; i++) {
        printf("Part %d, (x,y,t) is (%d, %d, %d)\n", i + 1, tmp[i].x, tmp[i].y, tmp[i].type);
    }
}


/**
 * 1. deformation model: use computed labels
 * 2. test image, should resize it to same size, model.maxsize
 */

int main(int argc, char **argv) {
    clock_t begin, end;
    double time_spent;
    begin = clock();
    //negative_feature();
    //print_feature(argc, argv);
    Model model;
    init_model(model);

    // training model
    cout << "training flag 1" << endl;
    training(model);

    // inference algorithm
    /*char file_name[50];
    Mat img;
    vector<Part> human;
    for (int f = 1; f <= model.positive_image_num; f++)  {
        cout << "start inference " << model.pos_file << "\n";
        sprintf(file_name, model.pos_file, f);
        img = imread(file_name);
        cout << file_name << "; img rows = " << img.cols << "; img cols = " << img.rows << endl;
        human = inference_origin(img, model);
        cout << "end inference\n";
        cout << "Print human part\n";
        print_human_part(human);
        print_groundtruth(model, f - 1);
        break;
    }
    free_model(model);
    */


    //vector<float> first(5, 2.0);
    //vector<float> second(5, 3.0);
    //float result = cblas_sdot(5, first.data(), 2, second.data(), 2);
    //cout << "result is " << result <<  endl;
    end = clock();
    cout << "Program spent time is " << (double)(end - begin) / CLOCKS_PER_SEC << "s"<< endl;
}
